#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <strings.h>

#include "common.h"
#include "memory.h"

/****
 * get_phys_page - Get the base address of a page given a physical address
 *
 * mem		- Memory map to get page from (mem != NULL)
 * addr		- Address of page (any offset will be ignored)
 * page		- A place to store a found page
 * create	- If true allocates space if page doesn't exist
 *
 * Returns E_OKAY on success, E_SYS_NOMEM if allocation failed or E_NOPAGE if
 * create == FALSE and the page doesn't already exist in memory.
 **/
static
phys_error
get_phys_page(	phys_mem mem,
				phys_addr addr,
				phys_byte **page,
				int create) {

#define table_check(t, o, s) 								\
	{	if(!((t)[o])) {										\
			if(!(create) || !((t)[o] = valloc(s))) 			\
				return (create ? E_SYS_NOMEM : E_NOPAGE);	\
			bzero((t)[o], s);								\
		}													\
	}

// When we run in 64 bit mode our address are much larger so we use more levels of 
// indirection.  This function effectively removes some of the pointer with specificity
// from the read/write functions.
#ifdef MIPS64
	uint16_t pdpt_idx, dir_idx, table_idx, page_idx;
	
	pdpt_idx	= (addr >> 55) & 0x01FF;
	dir_idx		= (addr >> 42) & 0x1FFF;
	table_idx	= (addr >> 29) & 0x1FFF;
	page_idx	= (addr >> 16) & 0x1FFF;
	
	table_check(mem, pdpt_idx, MEM_PAGE);
	table_check(mem[pdpt_idx], dir_idx, MEM_PAGE);
	table_check(mem[pdpt_idx][dir_idx], table_idx, MEM_PAGE);
	table_check(mem[pdpt_idx][dir_idx][table_idx], page_idx, MEM_PAGE);

	*page = mem[pdpt_idx][dir_idx][table_idx][page_idx];
#else /* !MIPS64 */
	uint16_t table_idx, page_idx;

	table_idx	= (addr >> 23) & 0x03FF;
	page_idx	= (addr >> 13) & 0x03FF;

	table_check(mem, table_idx, MEM_PAGE);
	table_check(mem, page_idx, MEM_PAGE);

	*page = mem[table_idx][page_idx];
#endif /* !MIPS64 */
#undef table_check
	return E_OKAY;
}

phys_error
free_phys_memory(	phys_mem mem) {
#ifdef MIPS64
	uint16_t pdpt, dir, table, page;

	for(pdpt=0; pdpt<0x200; pdpt++) {
		if(mem[pdpt]) {
			for(dir=0; dir<0x2000; dir++) {
				if(mem[pdpt][dir]) {
					for(table=0; table<0x2000; table++) {
						if(mem[pdpt][dir][table]) {
							free(mem[pdpt][dir][table]);	
						}
					}
					free(mem[pdpt][dir]);
				}
			}
			free(mem[pdpt]);
		}
	}
#else /* !MIPS64 */
	uint16_t table, page;
	for(table=0; table<0x0400; table++) {
		if(mem[table]) {
			for(page=0; page<0x0400; page++) {
				if(mem[table][page])
					free(mem[table][page]);
			}
			free(mem[table]);
		}
	}
#endif /* !MIPS64 */

	return E_OKAY;
}

phys_error
write_phys_memory(	phys_mem mem,
					phys_addr addr,
					uint8_t bytes,
					const void *v) {
	phys_error		err;
	phys_addr		offset, page1_len;
	phys_byte		*page1, *page2;
	const phys_byte	*b = (phys_byte *)v;

	// Can only write in powers of 2
	if(bytes & (bytes-1))
		return E_INVALID;

	// Attempt to fetch the starting page,
	//  if we can't even do that we're done
	if(err = get_phys_page(mem, addr, &page1, 1))
		return err;
	
	// determine the offset on page1 of the write,
	// PAGE_MASK is dependent on the size of phys_addr
	offset = addr & PAGE_MASK;
	
	// If we're properly aligned the write is all on one page,
	//  even if we aren't the write may still be on the same page
	if(!(addr & (bytes-1)) || !((offset+bytes) & ~PAGE_MASK)) {
		bcopy(b, page1+offset, bytes);
		return E_OKAY;
	}

	// We can span at most 1 page boundry because pages are
	// at LEAST 4KB and reads/writes are at most 255 bytes
	if(err = get_phys_page(mem, addr+bytes, &page2, 1))
		return err;
	
	// At the point we know that we are writing over a page
	// boundry, so the number of bytes our alignment is off
	// by is precisely the number of bytes over the boundry
	page1_len = bytes - (addr & (bytes-1));
	bcopy(b, page1+offset, page1_len);
	bcopy(b+page1_len, page2, bytes-page1_len);

	return E_OKAY;
}

phys_error
read_phys_memory(	phys_mem mem,
					phys_addr addr,
					uint8_t bytes,
					void *v) {
	phys_error	err;
	phys_addr	offset, page1_len;
	phys_byte	*page1, *page2;
	phys_byte *b = (phys_byte *)v;

	// Can only read in powers of 2
	if(bytes & (bytes-1))
		return E_INVALID;
	
	// Attempt to fetch the page that the address lies on
	//  if we can't fetch the page it's either because it
	//  has never been written, in which case we define say
	//  it's all zeros or there was an error (in which case
	//  we fail)
	if(err = get_phys_page(mem, addr, &page1, 0)) {
		if(err != E_NOPAGE)
			return err;
		page1 = NULL;
	}

	// determine the offset on page1 of the read, PAGE_MASK depends
	//  on the size of phys_addr and the layout of our tables
	offset = addr & PAGE_MASK;

	// If the address we're reading from is aligned to the number of
	// bytes we're reading then we won't span two pages, on the other
	// hand if it isn't we still may be on one page, so we check
	if(!(addr & (bytes-1)) || !((offset+bytes) & ~PAGE_MASK)) {
		// If page1 has been allocated perform the read, otherwise
		// simply return that the memory was 0s
		if(page1)
			bcopy(page1+offset, b, bytes);
		else
			bzero(b, bytes);
		return E_OKAY;
	}
	
	// Attempt to fetch the second page that our data spans
	// we know that pages are at LEAST 4KB and reads are at
	// most 255 bytes so they can span at most 1 page boundry
	if(err = get_phys_page(mem, addr+bytes, &page2, 0)) {
		if(err != E_NOPAGE)
			return err;
		page2 = NULL;
	}
	
	// Determine the number of bytes on the first page based
	// only on alignment (this is safe since we now know that
	// it must span a page boundry)
	page1_len = bytes - (addr & (bytes-1));

	// Read from page1, as above return zeros if it doesn't exist
	if(page1)
		bcopy(page1+offset, b, page1_len);
	else
		bzero(b, page1_len);

	// Ditto for page2
	if(page2)
		bcopy(page2, b+page1_len, bytes-page1_len);
	else
		bzero(b+page1_len, bytes-page1_len);

	return E_OKAY;
}


